#pragma once

#include <iostream>
#include <string>
#include <vector>

#include <boost/property_tree/info_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <ocs2_core/Types.h>
#include <ocs2_core/misc/LoadData.h>

namespace ocs2 {
    namespace quadrotor {
        struct QuadrotorParameters {
            
            scalar_t quadrotorMass_ = 1.0;
            scalar_t Thzz_ = 1.0;
            scalar_t Thxx_ = 1.0;
            scalar_t Thyy_ = 1.0;
            scalar_t gravity_ = 9.81;
        };

        inline QuadrotorParameters loadSettings(const std::string& filename, const std::string& fieldName = "QuadrotorParameters",
                                                bool verbose = true) {
            
            boost::property_tree::ptree pt;
            boost::property_tree::read_info(filename, pt);

            QuadrotorParameters settings;

            if (verbose) {
                std::cerr << "\n #### Quadrotor Parameters: ";
                std::cerr << "\n #### =========================================\n";
            }

            loadData::loadPtreeValue(pt, settings.quadrotorMass_, fieldName + ".quadrotorMass", verbose);
            loadData::loadPtreeValue(pt, settings.Thzz_, fieldName + ".Thzz", verbose);
            loadData::loadPtreeValue(pt, settings.Thxx_, fieldName + ".Thxx", verbose);
            loadData::loadPtreeValue(pt, settings.Thyy_, fieldName + ".Thyy", verbose);
            loadData::loadPtreeValue(pt, settings.gravity_, fieldName + ".gravity", verbose);

            if (verbose) {
                std::cerr << "#### =============================================" << std::endl;
            }

            return settings;
        }
    } // namespace quadrotor
} // namespace ocs2
